# -*- coding: utf-8 -*-
'''
Created on 21.06.2019

@author: yu03
'''

 
import matplotlib.pyplot as plt
import numpy as np
import FFT_Interpolation
from scipy import signal


j = complex(0, 1)
c = 3e8 # 光速 [m/s]
Lamda = 633e-9 # 光波长 [m]
Fc = c / Lamda # 光频率 [Hz]

screen_diameter = 2e-3
I_0 = 1
# dx = np.linspace(screen_diameter/2*(-1), screen_diameter/2, num=301)
dx = np.linspace(-1e-3, 1e-3, num=401)
dy = dx

V_x, V_y, V_z = 0.000, 0.000, 1 # Reference
V_x_2, V_y_2, Vz_2 = -0.001000001, -0.001, 1 # Measurement

tana = (V_x**2 + V_y**2)**0.5
tana_2 = (V_x_2**2 + V_y_2**2)**0.5

L = 0.4
D = 0.1
X, Y = np.meshgrid(dx, dy)
D_d = D - X * np.tan(V_x_2 - V_x) + Y * np.tan(V_y_2 - V_y)
L_d = L + X * V_x + Y * V_y

diff_L_d_2 = D_d + (L_d + D_d) * (1 - (V_x_2**2 + V_y_2**2)) / (1 + (V_x_2**2 + V_y_2**2)) - L_d * (1 - (V_x**2 + V_y**2)) / (1 + (V_x**2 + V_y**2))
Z = 2 * I_0 * (1 + np.cos(2 * np.pi * diff_L_d_2 / Lamda))

horizontal_centerline = Z[201]
print(horizontal_centerline)
# horizontal_centerline = horizontal_centerline * signal.gaussian(len(horizontal_centerline), std=len(horizontal_centerline)/4)
horizontal_centerline = horizontal_centerline - np.average(horizontal_centerline)
horizontal_freq, horizontal_phase = FFT_Interpolation.FFT_interpolation_boxcar(horizontal_centerline, screen_diameter/len(horizontal_centerline))[0:2]
horizontal_angle = V_x - Lamda * horizontal_freq / 2
horizontal_phase = horizontal_phase / 2 / np.pi * 360 ### in degree
if horizontal_phase < 0:
    horizontal_phase += 360
# horizontal_angle = (-1*Lamda*horizontal_freq + V_x*2/(1+tana**2)) / 2 * (1+tana_2**2)
print(horizontal_angle)
print(horizontal_phase)


hor_angles = []
hor_phases = []
# hor_angles_2 = []

horizontal_lines = Z[::2]
for line in horizontal_lines:
    line = line - np.average(line)
    line_freq, line_phase = FFT_Interpolation.FFT_interpolation_boxcar(line, screen_diameter/len(line))[0:2]
    
    line_angle = V_x - Lamda * line_freq / 2
#     line_angle_2 = (-1*Lamda*line_freq + V_x*2/(1+tana**2)) / 2 * (1+tana_2**2)

    line_phase = line_phase / 2 / np.pi * 360 ### in degree
    if line_phase < 0:
        line_phase += 360
    hor_angles.append(line_angle)
    hor_phases.append(line_phase)
#     hor_angles_2.append(line_angle_2)
    

if 0: ### Plot Pattern
    plt.figure('Pattern')
    c = plt.gca().pcolor(X*1000, Y*1000, Z, cmap='gray')
    plt.xlabel('dx [mm]')
    plt.ylabel('dy [mm]')
    plt.colorbar(c, ax=plt.gca())
    
plt.figure('Horizontal Line Analyze')
plt.subplot(2,1,1)
# plt.plot(horizontal_centerline)
plt.plot(hor_angles, marker=' ')
# plt.plot(hor_angles_2, color='red')
plt.subplot(2,1,2)
plt.plot(hor_phases)
plt.show()